import Jetson.GPIO as GPIO
import time

# 配置PWM引脚（根据您的实际接线修改）
PWM_1 = 15  # 水平舵机PWM信号引脚
PWM_2 = 32  # 垂直舵机PWM信号引脚
 
def pwm_test():
    try:
        # 初始化GPIO
        GPIO.setmode(GPIO.BOARD)
        GPIO.setwarnings(False)

        GPIO.setup(PWM_1, GPIO.OUT)
        GPIO.setup(PWM_2, GPIO.OUT)
        
        # 初始化PWM通道（频率50Hz）
        pan_pwm = GPIO.PWM(PWM_1, 50)
        tilt_pwm = GPIO.PWM(PWM_2, 50)

        pan_pwm.start(7.5)
        tilt_pwm.start(7.5)
        # 测试序列
        angles = [0, 90, 180]  # 水平舵机测试角度
        angles_v = [60, 90, 120]  # 垂直舵机测试角度
        
        for i in range(3):
            # 设置水平舵机角度
            duty_cycle = 2.5 + (angles[i] / 180) * 10
            pan_pwm.ChangeDutyCycle(duty_cycle / 100)
            
            # 设置垂直舵机角度
            duty_cycle_v = 2.5 + (angles_v[i] / 180) * 10
            tilt_pwm.ChangeDutyCycle(duty_cycle_v / 100)
            
            print(f"\n测试步骤 {i+1}:")
            print(f"水平舵机应转动到: {angles[i]}°")
            print(f"垂直舵机应转动到: {angles_v[i]}°")
            
            time.sleep(3)  # 保持角度3秒
            
    except KeyboardInterrupt:
        print("\n测试中断，正在清理GPIO...")
    except Exception as e:
        print(f"测试出错: {str(e)}")
    finally:
        # 清理GPIO
        pan_pwm.stop()
        tilt_pwm.stop()
        print("GPIO已清理")
 
if __name__ == "__main__":
    pwm_test()